#include "LooseCoupling.h"
#include "timeSync.h"
#include <string.h>

CLCKF::CLCKF(const CVec3 &att0, const CVec3 &vn0, const CVec3 &pos0)
{
    clear();
    Qt(0) = math::sq(imuParam.ARW[0] * glv.dpsh);
    Qt(1) = math::sq(imuParam.ARW[1] * glv.dpsh);
    Qt(2) = math::sq(imuParam.ARW[2] * glv.dpsh);
    Qt(3) = math::sq(imuParam.VRW[0] * glv.ugpshz);
    Qt(4) = math::sq(imuParam.VRW[1] * glv.ugpshz);
    Qt(5) = math::sq(imuParam.VRW[2] * glv.ugpshz);

    Pk(1,1) = Pk(0,0) = math::sq(0.5 * glv.d2r);            // phiEN 0.5°
    Pk(2,2) = math::sq(1.5 * glv.d2r);                      // phiU  1.0°
    Pk(4,4) = Pk(3,3) = math::sq(0.15);                     // velEN 0.15m/s
    Pk(5,5) = math::sq(0.15);                               // velU  0.15m/s
#if (USE_LLH_Projection)
    Pk(7,7) = Pk(6,6) = math::sq(1.0);
#else
    Pk(7,7) = Pk(6,6) = math::sq(1.0 / glv.Re);
#endif
    Pk(8,8) = math::sq(3.0);
    Pk(11,11) = Pk(10,10) = Pk(9,9) = math::sq(imuParam.gyrBiasRepeatability * glv.dph);
    Pk(14,14) = Pk(13,13) = Pk(12,12) = math::sq(imuParam.accBias[0] * glv.ug);

    Rk(0) = Rk(1) = 0;
    Rk(2) = math::sq(1.0 * glv.d2r);
    Rk(4) = Rk(3) = math::sq(0.1);
    Rk(5) = math::sq(0.1);
    Rk(7) = Rk(6) = math::sq(1.0/glv.Re);
    Rk(8) = math::sq(3.0);
    init(att0, vn0, pos0, 0);
}

void CLCKF::init(const CVec3 &att0, const CVec3 &vn0, const CVec3 &pos0, double tk)
{
    this->tk = this->tk_1 = tk, this->dt = 0;
    sins.init(att0, vn0, pos0, tk);
}

void CLCKF::sins_update(const double *gyr, const double *acc, double tk, int nSamples)
{
    sins.update(gyr, acc, tk, nSamples);

    if (outLever.isZero())
    {
        avpt.push(tk, sins.att, sins.vn, sins.pos);
    }
    else
    {
        sins.outerLever(outLever);
        avpt.push(tk, sins.att, sins.vnL, sins.posL);
    }
}

void CLCKF::time_update(double tk)
{
    this->tk = tk;
    this->dt = this->tk - this->tk_1;
    this->tk_1 = this->tk;

    predict_xkpk(xk.data(), Pk.data(), Qt.data(), sins.Cnb.data(), sins.eth.Mpvv.data(),
                 sins.fn.data(), betaGyr.data(), betaAcc.data(), this->dt, 
                 xkk_1.data(), Pkk_1.data() );
    
    /* xk/Pk constrain */
    for (int i = 0; i < 15; i++)
    {
        xk(i) = math::constrain(xkk_1(i), -xkMax(i), xkMax(i));
        if(Pkk_1(i,i) < pkMin(i))
        {
            Pkk_1(i,i) = pkMin(i);
        }
        else if(Pkk_1(i,i) > pkMax(i))
        {
            double sqf = sqrt(pkMax(i) / Pkk_1(i, i)) * 0.9;
            for (int j = 1; j<= i; j++)
            {
                Pkk_1(j,i) = Pkk_1(j,i) * sqf;
            }
        }
    }
    
    /* Pk对称化 */
    Pk.triangularView<Eigen::Upper>() = Pkk_1.triangularView<Eigen::Upper>();
    Pk.triangularView<Eigen::StrictlyLower>() = Pk.triangularView<Eigen::StrictlyUpper>().transpose(); 
}

void CLCKF::gnssMeas_update(double tk, const CVec3 &vn, const CVec3 &pos, bool isHgtValid, const double *dualAntennaYaw)
{
    avpt.interp(tk);

    double zk[7];
    zk[1] = avpt.vni(0) - vn(0),   zk[2] = avpt.vni(1) - vn(1),  zk[3] = avpt.vni(2) - vn(2);
    zk[4] = avpt.posi(0) - pos(0), zk[5] = avpt.posi(1) - pos(1);
   
    obs_tGnss[0] = tk;
    obs_tGnss[1] = vn(0), obs_tGnss[2] = vn(1), obs_tGnss[3] = vn(2);
    obs_tGnss[4] = pos(0), obs_tGnss[5] = pos(1), obs_tGnss[6] = pos(2);

    int stateIdx, stateIdxMax;
    if (dualAntennaYaw != nullptr)
    {
        double yawErr = avpt.atti(2) - *dualAntennaYaw;
        if (yawErr >= math::PI)
            yawErr -= 2 * math::PI;
        else if (yawErr <= -math::PI)
            yawErr += 2 * math::PI;
        zk[0] = -yawErr;
        stateIdx = 2;  

        obs_tGnssYaw[0] = tk;
        obs_tGnssYaw[1] = *dualAntennaYaw;
    }
    else
    {
        zk[0] = 0;
        stateIdx = 3;
    }

    if (isHgtValid)
    {
        zk[6] = avpt.posi(2) - pos(2);
        stateIdxMax = 8;
    }
    else
    {
        zk[6] = 0;
        stateIdxMax = 7;
    }

    for ( int obsIdx = stateIdx - 2; stateIdx <= stateIdxMax; stateIdx++, obsIdx++)
    {
        innov_var(stateIdx) = Pk(stateIdx,stateIdx) + Rk(stateIdx);
        Kk = Pk.col(stateIdx) * (1.0 / innov_var(stateIdx));
        innov(stateIdx) = zk[obsIdx] - xk(stateIdx);

        Pk = Pk - Kk * Pk.row(stateIdx);
        for (int k = 0; k < 15; k++)
        {
            if(Pk(k,k) <= 0)        // 负定
            {
                requestToResetFilter = true;
                pkNonPositiveDefiniteCnt++;
                break;
            }
        }
        if(requestToResetFilter){ break; }
        xk = xk + Kk * innov(stateIdx);
    }

    if (requestToResetFilter)
    {
        resetFilter();
    }
}

void CLCKF::magnMeas_update(double tk, const CVec3 &Magn)
{
    avpt.interp(tk);
}

void CLCKF::baroMeas_update(double tk, double baroHgt)
{
    avpt.interp(tk);
}

void CLCKF::feedback(uint32_t bits)
{
    double *pxk = xk.data();
    Eigen::Map<CVec3> phi (pxk);      sins.qnb = math::qdelphi(sins.qnb, phi);
    Eigen::Map<CVec3> dvn (pxk+3);    sins.vn  = sins.vn - dvn;
    Eigen::Map<CVec3> dpos(pxk+6);    sins.pos = sins.pos - dpos;
    Eigen::Map<CVec3> eb  (pxk+9);    sins.eb  = sins.eb + eb;
    Eigen::Map<CVec3> db  (pxk+12);   sins.db  = sins.db + db;
    xk.setZero();
}

void CLCKF::clear()
{
    dt = 0;
    tk = 0, tk_1 = 0;
    outLever.setZero();
    betaAcc.setZero(), betaGyr.setZero();
    xk.setZero(), xkk_1.setZero(), Kk.setZero(), Pkk_1.setZero();
    Qt.setZero(), Rk.setZero(), Pk.setZero();

    xkMax.setOnes() *= 1e3;
    pkMin.setZero(), pkMax.setOnes() *= 1e3;

    innov.setZero();
    innov_var.setZero();
    rho.setOnes(), lambda.setZero(), TDk.setZero();

    resetFilterCnt = 0;
    pkNonPositiveDefiniteCnt = 0;
    requestToResetFilter = false;

    memset((void *)obs_tGnssYaw, 0, 2 * sizeof(double));
    memset((void *)obs_tGnss, 0, 7 * sizeof(double));
    memset((void *)obs_tMagYaw, 0, 2 * sizeof(double));
    memset((void *)obs_tBaroHgt, 0, 2 * sizeof(double));
}

void CLCKF::resetFilter(void)
{
    resetFilterCnt++;
    requestToResetFilter = false;
}

void CLCKF::resetAtt(double pit, double pitNoiseStd, double rol, double rolNoiseStd, double yaw, double yawNoiseStd)
{
}

void CLCKF::resetYaw(double yaw, double yawNoiseStd)
{
}

void CLCKF::resetVelNE(double velN, double vNNoiseStd, double velE, double vENoiseStd)
{
}

void CLCKF::resetVelU(double velU, double vUNoiseStd)
{
}

void CLCKF::resetPosNE(double posN, double posNNoiseStd, double posE, double posENoiseStd)
{
}

void CLCKF::resetPosU(double posU, double posUNoiseStd)
{
}

void CLCKF::predict_xkpk(const double *xk, const double *Pk,
                  const double *Qt, const double *Cnb,
                  const double *Mpvv, const double *fn,
                  const double *betaGyr, const double *betaAcc, double dt,
                  double *xkk_1, double *Pkk_1)
{
    double Xkk_1_tmp;
    double b_Xkk_1_tmp;
    double b_ct_tmp;
    double c_Xkk_1_tmp;
    double c_ct_tmp;
    double ct_idx_217;
    double ct_idx_228;
    double ct_idx_239;
    double ct_idx_250;
    double ct_idx_261;
    double ct_idx_272;
    double ct_idx_283;
    double ct_idx_294;
    double ct_idx_305;
    double ct_idx_316;
    double ct_idx_328;
    double ct_idx_335;
    double ct_idx_336;
    double ct_idx_337;
    double ct_idx_338;
    double ct_idx_340;
    double ct_idx_341;
    double ct_idx_342;
    double ct_idx_343;
    double ct_idx_344;
    double ct_idx_368;
    double ct_idx_369;
    double ct_idx_370;
    double ct_idx_371;
    double ct_idx_372;
    double ct_idx_373;
    double ct_idx_374;
    double ct_idx_375;
    double ct_idx_376;
    double ct_idx_377;
    double ct_idx_378;
    double ct_idx_379;
    double ct_idx_380;
    double ct_idx_381;
    double ct_idx_382;
    double ct_idx_383;
    double ct_idx_385;
    double ct_idx_391;
    double ct_idx_392;
    double ct_idx_394;
    double ct_idx_395;
    double ct_idx_396;
    double ct_idx_398;
    double ct_idx_399;
    double ct_idx_401;
    double ct_idx_402;
    double ct_idx_403;
    double ct_idx_407;
    double ct_idx_411;
    double ct_idx_414;
    double ct_idx_417;
    double ct_idx_418;
    double ct_idx_420;
    double ct_idx_429;
    double ct_tmp;
    double d_ct_tmp;
    double e_ct_tmp;
    double f_ct_tmp;
    double t267;
    double t268;
    double t269;
    double t270;
    double t271;
    double t272;
    double t273;
    double t274;
    double t275;
    double t276;
    double t277;
    double t278;
    double t279;
    double t280;
    double t281;
    double t282;
    double t283;
    double t284;
    double t285;
    double t286;
    double t287;
    double t288;
    double t289;
    double t290;
    double t291;
    double t292;
    double t293;
    double t294;
    double t295;
    double t296;
    double t297;
    double t298;
    double t299;
    double t300;
    double t301;
    double t302;
    double t303;
    double t304;
    double t305;
    double t306;

    double dt_Cnb[9];
    dt_Cnb[0] = Cnb[0] * dt;
    dt_Cnb[1] = Cnb[1] * dt;
    dt_Cnb[2] = Cnb[2] * dt;
    dt_Cnb[3] = Cnb[3] * dt;
    dt_Cnb[4] = Cnb[4] * dt;
    dt_Cnb[5] = Cnb[5] * dt;
    dt_Cnb[6] = Cnb[6] * dt;
    dt_Cnb[7] = Cnb[7] * dt;
    dt_Cnb[8] = Cnb[8] * dt;

    double dt_fnsf[3];
    dt_fnsf[0] = fn[0] * dt;
    dt_fnsf[1] = fn[1] * dt;
    dt_fnsf[2] = fn[2] * dt;

    double dt_betaAcc[3];
    dt_betaAcc[0] = betaAcc[0] * dt;
    dt_betaAcc[1] = betaAcc[1] * dt;
    dt_betaAcc[2] = betaAcc[2] * dt;

    double dt_betaGyr[3];
    dt_betaGyr[0] = betaGyr[0] * dt;
    dt_betaGyr[1] = betaGyr[1] * dt;
    dt_betaGyr[2] = betaGyr[2] * dt;

    /*     This function was generated by the Symbolic Math Toolbox version 24.2.
     */
    /*     2025-06-24 20:55:19 */
    ct_idx_217 = dt_Cnb[1] * Pk[24];
    ct_idx_228 = dt_Cnb[3] * Pk[10];
    ct_idx_239 = dt_Cnb[1] * Pk[39];
    ct_idx_250 = dt_Cnb[3] * Pk[25];
    ct_idx_261 = dt_Cnb[1] * Pk[54];
    ct_idx_272 = dt_Cnb[3] * Pk[40];
    ct_idx_283 = dt_Cnb[1] * Pk[69];
    ct_idx_294 = dt_Cnb[3] * Pk[55];
    ct_idx_305 = dt_Cnb[1] * Pk[84];
    ct_idx_316 = dt_Cnb[3] * Pk[70];
    ct_idx_328 = dt_Cnb[3] * Pk[85];
    ct_idx_335 = Cnb[0] * Cnb[0];
    ct_idx_336 = Cnb[3] * Cnb[3];
    ct_idx_337 = Cnb[6] * Cnb[6];
    ct_idx_338 = Cnb[1] * Cnb[1];
    ct_idx_340 = Cnb[4] * Cnb[4];
    ct_idx_341 = Cnb[7] * Cnb[7];
    ct_idx_342 = Cnb[2] * Cnb[2];
    ct_idx_343 = Cnb[5] * Cnb[5];
    ct_idx_344 = Cnb[8] * Cnb[8];
    ct_idx_368 = dt_Cnb[2] * Pk[39];
    ct_idx_369 = dt_Cnb[4] * Pk[25];
    ct_idx_370 = dt_Cnb[6] * Pk[11];
    ct_idx_371 = dt_Cnb[2] * Pk[54];
    ct_idx_372 = dt_Cnb[4] * Pk[40];
    ct_idx_373 = dt_Cnb[6] * Pk[26];
    ct_idx_374 = dt_Cnb[0] * Pk[9];
    ct_idx_375 = dt_Cnb[2] * Pk[69];
    ct_idx_376 = dt_Cnb[4] * Pk[55];
    ct_idx_377 = dt_Cnb[6] * Pk[41];
    ct_idx_378 = dt_Cnb[2] * Pk[84];
    ct_idx_379 = dt_Cnb[4] * Pk[70];
    ct_idx_380 = dt_Cnb[6] * Pk[56];
    ct_idx_381 = dt_Cnb[4] * Pk[85];
    ct_idx_382 = dt_Cnb[6] * Pk[71];
    ct_idx_383 = dt_Cnb[6] * Pk[86];
    ct_idx_385 = dt_Cnb[0] * Pk[24];
    ct_idx_391 = dt_Cnb[5] * Pk[40];
    ct_idx_392 = dt_Cnb[7] * Pk[26];
    ct_idx_394 = dt_Cnb[5] * Pk[55];
    ct_idx_395 = dt_Cnb[7] * Pk[41];
    ct_idx_396 = dt_Cnb[0] * Pk[39];
    ct_idx_398 = dt_Cnb[5] * Pk[70];
    ct_idx_399 = dt_Cnb[7] * Pk[56];
    ct_idx_401 = dt_Cnb[5] * Pk[85];
    ct_idx_402 = dt_Cnb[7] * Pk[71];
    ct_idx_403 = dt_Cnb[7] * Pk[86];
    ct_idx_407 = dt_Cnb[0] * Pk[54];
    ct_idx_411 = dt_Cnb[8] * Pk[41];
    ct_idx_414 = dt_Cnb[8] * Pk[56];
    ct_idx_417 = dt_Cnb[8] * Pk[71];
    ct_idx_418 = dt_Cnb[0] * Pk[69];
    ct_idx_420 = dt_Cnb[8] * Pk[86];
    ct_idx_429 = dt_Cnb[0] * Pk[84];
    xkk_1[0] =
        ((xk[0] - dt_Cnb[0] * xk[9]) - dt_Cnb[3] * xk[10]) - dt_Cnb[6] * xk[11];
    xkk_1[1] =
        ((xk[1] - dt_Cnb[1] * xk[9]) - dt_Cnb[4] * xk[10]) - dt_Cnb[7] * xk[11];
    xkk_1[2] =
        ((xk[2] - dt_Cnb[2] * xk[9]) - dt_Cnb[5] * xk[10]) - dt_Cnb[8] * xk[11];
    xkk_1[3] = ((((xk[3] + dt_Cnb[0] * xk[12]) + dt_Cnb[3] * xk[13]) +
                 dt_Cnb[6] * xk[14]) +
                dt_fnsf[1] * xk[2]) -
               xk[1] * dt_fnsf[2];
    xkk_1[4] = ((((xk[4] + dt_Cnb[1] * xk[12]) + dt_Cnb[4] * xk[13]) +
                 dt_Cnb[7] * xk[14]) -
                dt_fnsf[0] * xk[2]) +
               xk[0] * dt_fnsf[2];
    xkk_1[5] = ((((xk[5] + dt_Cnb[2] * xk[12]) + dt_Cnb[5] * xk[13]) +
                 dt_Cnb[8] * xk[14]) +
                dt_fnsf[0] * xk[1]) -
               xk[0] * dt_fnsf[1];
    Xkk_1_tmp = Mpvv[0] * dt;
    xkk_1[6] = xk[6] + Xkk_1_tmp * xk[4];
    b_Xkk_1_tmp = Mpvv[1] * dt;
    xkk_1[7] = xk[7] + b_Xkk_1_tmp * xk[3];
    c_Xkk_1_tmp = Mpvv[2] * dt;
    xkk_1[8] = xk[8] + c_Xkk_1_tmp * xk[5];
    xkk_1[9] = -(dt_betaGyr[0] - 1.0) * xk[9];
    xkk_1[10] = -(dt_betaGyr[1] - 1.0) * xk[10];
    xkk_1[11] = -(dt_betaGyr[2] - 1.0) * xk[11];
    xkk_1[12] = -(dt_betaAcc[0] - 1.0) * xk[12];
    xkk_1[13] = -(dt_betaAcc[1] - 1.0) * xk[13];
    xkk_1[14] = -(dt_betaAcc[2] - 1.0) * xk[14];
    t267 = ((ct_idx_228 + ct_idx_374) + ct_idx_370) - Pk[0];
    t268 = ((ct_idx_250 + ct_idx_385) + ct_idx_373) - Pk[15];
    t269 = ((ct_idx_272 + ct_idx_396) + ct_idx_377) - Pk[30];
    t270 =
        ((dt_Cnb[4] * Pk[10] + dt_Cnb[1] * Pk[9]) + dt_Cnb[7] * Pk[11]) - Pk[1];
    t271 = ((ct_idx_217 + ct_idx_369) + ct_idx_392) - Pk[16];
    t272 = ((ct_idx_239 + ct_idx_372) + ct_idx_395) - Pk[31];
    t273 =
        ((dt_Cnb[2] * Pk[9] + dt_Cnb[5] * Pk[10]) + dt_Cnb[8] * Pk[11]) - Pk[2];
    t274 =
        ((dt_Cnb[2] * Pk[24] + dt_Cnb[5] * Pk[25]) + dt_Cnb[8] * Pk[26]) - Pk[17];
    t275 = ((ct_idx_368 + ct_idx_391) + ct_idx_411) - Pk[32];
    t276 = ((dt_Cnb[3] * Pk[145] + dt_Cnb[0] * Pk[144]) + dt_Cnb[6] * Pk[146]) -
           Pk[135];
    t277 = ((dt_Cnb[3] * Pk[160] + dt_Cnb[0] * Pk[159]) + dt_Cnb[6] * Pk[161]) -
           Pk[150];
    t278 = ((dt_Cnb[3] * Pk[175] + dt_Cnb[0] * Pk[174]) + dt_Cnb[6] * Pk[176]) -
           Pk[165];
    t279 = ((dt_Cnb[3] * Pk[190] + dt_Cnb[0] * Pk[189]) + dt_Cnb[6] * Pk[191]) -
           Pk[180];
    t280 = ((dt_Cnb[3] * Pk[205] + dt_Cnb[0] * Pk[204]) + dt_Cnb[6] * Pk[206]) -
           Pk[195];
    t281 = ((dt_Cnb[3] * Pk[220] + dt_Cnb[0] * Pk[219]) + dt_Cnb[6] * Pk[221]) -
           Pk[210];
    t282 = ((dt_Cnb[4] * Pk[145] + dt_Cnb[1] * Pk[144]) + dt_Cnb[7] * Pk[146]) -
           Pk[136];
    t283 = ((dt_Cnb[4] * Pk[160] + dt_Cnb[1] * Pk[159]) + dt_Cnb[7] * Pk[161]) -
           Pk[151];
    t284 = ((dt_Cnb[4] * Pk[175] + dt_Cnb[1] * Pk[174]) + dt_Cnb[7] * Pk[176]) -
           Pk[166];
    t285 = ((dt_Cnb[4] * Pk[190] + dt_Cnb[1] * Pk[189]) + dt_Cnb[7] * Pk[191]) -
           Pk[181];
    t286 = ((dt_Cnb[4] * Pk[205] + dt_Cnb[1] * Pk[204]) + dt_Cnb[7] * Pk[206]) -
           Pk[196];
    t287 = ((dt_Cnb[4] * Pk[220] + dt_Cnb[1] * Pk[219]) + dt_Cnb[7] * Pk[221]) -
           Pk[211];
    t288 = ((dt_Cnb[5] * Pk[145] + dt_Cnb[2] * Pk[144]) + dt_Cnb[8] * Pk[146]) -
           Pk[137];
    t289 = ((dt_Cnb[2] * Pk[159] + dt_Cnb[5] * Pk[160]) + dt_Cnb[8] * Pk[161]) -
           Pk[152];
    t290 = ((dt_Cnb[2] * Pk[174] + dt_Cnb[5] * Pk[175]) + dt_Cnb[8] * Pk[176]) -
           Pk[167];
    t291 = ((dt_Cnb[2] * Pk[189] + dt_Cnb[5] * Pk[190]) + dt_Cnb[8] * Pk[191]) -
           Pk[182];
    t292 = ((dt_Cnb[2] * Pk[204] + dt_Cnb[5] * Pk[205]) + dt_Cnb[8] * Pk[206]) -
           Pk[197];
    t293 = ((dt_Cnb[2] * Pk[219] + dt_Cnb[5] * Pk[220]) + dt_Cnb[8] * Pk[221]) -
           Pk[212];
    t294 = ((((Pk[3] + dt_Cnb[0] * Pk[12]) + dt_Cnb[3] * Pk[13]) +
             dt_Cnb[6] * Pk[14]) +
            dt_fnsf[1] * Pk[2]) -
           Pk[1] * dt_fnsf[2];
    t295 = ((((Pk[18] + dt_Cnb[0] * Pk[27]) + dt_Cnb[3] * Pk[28]) +
             dt_Cnb[6] * Pk[29]) +
            dt_fnsf[1] * Pk[17]) -
           dt_fnsf[2] * Pk[16];
    t296 = ((((Pk[33] + dt_Cnb[0] * Pk[42]) + dt_Cnb[3] * Pk[43]) +
             dt_Cnb[6] * Pk[44]) +
            dt_fnsf[1] * Pk[32]) -
           dt_fnsf[2] * Pk[31];
    t297 = ((((Pk[4] + dt_Cnb[1] * Pk[12]) + dt_Cnb[4] * Pk[13]) +
             dt_Cnb[7] * Pk[14]) +
            Pk[0] * dt_fnsf[2]) -
           dt_fnsf[0] * Pk[2];
    t298 = ((((Pk[183] + dt_Cnb[0] * Pk[192]) + dt_Cnb[3] * Pk[193]) +
             dt_Cnb[6] * Pk[194]) +
            dt_fnsf[1] * Pk[182]) -
           dt_fnsf[2] * Pk[181];
    t299 = ((((Pk[198] + dt_Cnb[0] * Pk[207]) + dt_Cnb[3] * Pk[208]) +
             dt_Cnb[6] * Pk[209]) +
            dt_fnsf[1] * Pk[197]) -
           dt_fnsf[2] * Pk[196];
    t300 = ((((Pk[213] + dt_Cnb[0] * Pk[222]) + dt_Cnb[3] * Pk[223]) +
             dt_Cnb[6] * Pk[224]) +
            dt_fnsf[1] * Pk[212]) -
           dt_fnsf[2] * Pk[211];
    t301 = ((((Pk[184] + dt_Cnb[1] * Pk[192]) + dt_Cnb[4] * Pk[193]) +
             dt_Cnb[7] * Pk[194]) +
            dt_fnsf[2] * Pk[180]) -
           dt_fnsf[0] * Pk[182];
    t302 = ((((Pk[199] + dt_Cnb[1] * Pk[207]) + dt_Cnb[4] * Pk[208]) +
             dt_Cnb[7] * Pk[209]) +
            dt_fnsf[2] * Pk[195]) -
           dt_fnsf[0] * Pk[197];
    t303 = ((((Pk[214] + dt_Cnb[1] * Pk[222]) + dt_Cnb[4] * Pk[223]) +
             dt_Cnb[7] * Pk[224]) +
            dt_fnsf[2] * Pk[210]) -
           dt_fnsf[0] * Pk[212];
    t304 = ((((Pk[185] + dt_Cnb[2] * Pk[192]) + dt_Cnb[5] * Pk[193]) +
             dt_Cnb[8] * Pk[194]) +
            dt_fnsf[0] * Pk[181]) -
           dt_fnsf[1] * Pk[180];
    t305 = ((((Pk[200] + dt_Cnb[2] * Pk[207]) + dt_Cnb[5] * Pk[208]) +
             dt_Cnb[8] * Pk[209]) +
            dt_fnsf[0] * Pk[196]) -
           dt_fnsf[1] * Pk[195];
    t306 = ((((Pk[215] + dt_Cnb[2] * Pk[222]) + dt_Cnb[5] * Pk[223]) +
             dt_Cnb[8] * Pk[224]) +
            dt_fnsf[0] * Pk[211]) -
           dt_fnsf[1] * Pk[210];
    ct_tmp = Qt[0] * dt;
    b_ct_tmp = Qt[1] * dt;
    c_ct_tmp = Qt[2] * dt;
    Pkk_1[0] = ((((((((Pk[0] - ct_idx_374) - ct_idx_228) - ct_idx_370) +
                    dt_Cnb[0] * t276) +
                   dt_Cnb[3] * t277) +
                  dt_Cnb[6] * t278) +
                 ct_tmp * ct_idx_335) +
                b_ct_tmp * ct_idx_336) +
               c_ct_tmp * ct_idx_337;
    memset(&Pkk_1[1], 0, 14U * sizeof(double));
    d_ct_tmp = Cnb[0] * Cnb[1];
    e_ct_tmp = Cnb[3] * Cnb[4];
    f_ct_tmp = Cnb[6] * Cnb[7];
    Pkk_1[15] = ((((((((Pk[15] - ct_idx_385) - ct_idx_250) - ct_idx_373) +
                     dt_Cnb[1] * t276) +
                    dt_Cnb[4] * t277) +
                   dt_Cnb[7] * t278) +
                  d_ct_tmp * Qt[0] * dt) +
                 e_ct_tmp * Qt[1] * dt) +
                f_ct_tmp * Qt[2] * dt;
    Pkk_1[16] = ((((((((Pk[16] - ct_idx_217) - ct_idx_369) - ct_idx_392) +
                     dt_Cnb[1] * t282) +
                    dt_Cnb[4] * t283) +
                   dt_Cnb[7] * t284) +
                  ct_tmp * ct_idx_338) +
                 b_ct_tmp * ct_idx_340) +
                c_ct_tmp * ct_idx_341;
    memset(&Pkk_1[17], 0, 13U * sizeof(double));
    ct_idx_369 = Cnb[0] * Cnb[2];
    ct_idx_373 = Cnb[3] * Cnb[5];
    ct_idx_385 = Cnb[6] * Cnb[8];
    Pkk_1[30] = ((((((((Pk[30] - ct_idx_396) - ct_idx_272) - ct_idx_377) +
                     dt_Cnb[2] * t276) +
                    dt_Cnb[5] * t277) +
                   dt_Cnb[8] * t278) +
                  ct_idx_369 * Qt[0] * dt) +
                 ct_idx_373 * Qt[1] * dt) +
                ct_idx_385 * Qt[2] * dt;
    ct_idx_374 = Cnb[1] * Cnb[2];
    ct_idx_217 = Cnb[4] * Cnb[5];
    ct_idx_250 = Cnb[7] * Cnb[8];
    Pkk_1[31] = ((((((((Pk[31] - ct_idx_239) - ct_idx_372) - ct_idx_395) +
                     dt_Cnb[2] * t282) +
                    dt_Cnb[5] * t283) +
                   dt_Cnb[8] * t284) +
                  ct_idx_374 * Qt[0] * dt) +
                 ct_idx_217 * Qt[1] * dt) +
                ct_idx_250 * Qt[2] * dt;
    Pkk_1[32] = ((((((((Pk[32] - ct_idx_368) - ct_idx_391) - ct_idx_411) +
                     dt_Cnb[2] * t288) +
                    dt_Cnb[5] * t289) +
                   dt_Cnb[8] * t290) +
                  ct_tmp * ct_idx_342) +
                 b_ct_tmp * ct_idx_343) +
                c_ct_tmp * ct_idx_344;
    memset(&Pkk_1[33], 0, 12U * sizeof(double));
    Pkk_1[45] = (((((((Pk[45] - ct_idx_407) - ct_idx_294) - ct_idx_380) -
                    dt_Cnb[0] * t279) -
                   dt_Cnb[3] * t280) -
                  dt_Cnb[6] * t281) -
                 dt_fnsf[1] * t269) +
                dt_fnsf[2] * t268;
    Pkk_1[46] = (((((((Pk[46] - ct_idx_261) - ct_idx_376) - ct_idx_399) -
                    dt_Cnb[0] * t285) -
                   dt_Cnb[3] * t286) -
                  dt_Cnb[6] * t287) -
                 dt_fnsf[1] * t272) +
                dt_fnsf[2] * t271;
    Pkk_1[47] = (((((((Pk[47] - ct_idx_371) - ct_idx_394) - ct_idx_414) -
                    dt_Cnb[0] * t291) -
                   dt_Cnb[3] * t292) -
                  dt_Cnb[6] * t293) -
                 dt_fnsf[1] * t275) +
                dt_fnsf[2] * t274;
    ct_tmp = Qt[3] * dt;
    b_ct_tmp = Qt[4] * dt;
    c_ct_tmp = Qt[5] * dt;
    ct_idx_370 = ((((Pk[48] + dt_Cnb[0] * Pk[57]) + dt_Cnb[3] * Pk[58]) +
                   dt_Cnb[6] * Pk[59]) +
                  dt_fnsf[1] * Pk[47]) -
                 dt_fnsf[2] * Pk[46];
    Pkk_1[48] = (((((((ct_idx_370 + dt_Cnb[0] * t298) + dt_Cnb[3] * t299) +
                     dt_Cnb[6] * t300) +
                    dt_fnsf[1] * t296) -
                   dt_fnsf[2] * t295) +
                  ct_tmp * ct_idx_335) +
                 b_ct_tmp * ct_idx_336) +
                c_ct_tmp * ct_idx_337;
    memset(&Pkk_1[49], 0, 11U * sizeof(double));
    Pkk_1[60] = (((((((Pk[60] - ct_idx_418) - ct_idx_316) - ct_idx_382) -
                    dt_Cnb[1] * t279) -
                   dt_Cnb[4] * t280) -
                  dt_Cnb[7] * t281) +
                 dt_fnsf[0] * t269) -
                dt_fnsf[2] * t267;
    Pkk_1[61] = (((((((Pk[61] - ct_idx_283) - ct_idx_379) - ct_idx_402) -
                    dt_Cnb[1] * t285) -
                   dt_Cnb[4] * t286) -
                  dt_Cnb[7] * t287) +
                 dt_fnsf[0] * t272) -
                dt_fnsf[2] * t270;
    Pkk_1[62] = (((((((Pk[62] - ct_idx_375) - ct_idx_398) - ct_idx_417) -
                    dt_Cnb[1] * t291) -
                   dt_Cnb[4] * t292) -
                  dt_Cnb[7] * t293) +
                 dt_fnsf[0] * t275) -
                dt_fnsf[2] * t273;
    ct_idx_228 = ((((Pk[63] + dt_Cnb[0] * Pk[72]) + dt_Cnb[3] * Pk[73]) +
                   dt_Cnb[6] * Pk[74]) +
                  dt_fnsf[1] * Pk[62]) -
                 dt_fnsf[2] * Pk[61];
    Pkk_1[63] = (((((((ct_idx_228 + dt_Cnb[1] * t298) + dt_Cnb[4] * t299) +
                     dt_Cnb[7] * t300) -
                    dt_fnsf[0] * t296) +
                   dt_fnsf[2] * t294) +
                  d_ct_tmp * Qt[3] * dt) +
                 e_ct_tmp * Qt[4] * dt) +
                f_ct_tmp * Qt[5] * dt;
    d_ct_tmp = ((((Pk[64] + dt_Cnb[1] * Pk[72]) + dt_Cnb[4] * Pk[73]) +
                 dt_Cnb[7] * Pk[74]) +
                dt_fnsf[2] * Pk[60]) -
               dt_fnsf[0] * Pk[62];
    Pkk_1[64] =
        (((((((d_ct_tmp + dt_Cnb[1] * t301) + dt_Cnb[4] * t302) +
             dt_Cnb[7] * t303) +
            dt_fnsf[2] * t297) -
           dt_fnsf[0] * (((((Pk[34] + dt_Cnb[1] * Pk[42]) + dt_Cnb[4] * Pk[43]) +
                           dt_Cnb[7] * Pk[44]) +
                          dt_fnsf[2] * Pk[30]) +
                         dt_fnsf[0] * -Pk[32])) +
          ct_tmp * ct_idx_338) +
         b_ct_tmp * ct_idx_340) +
        c_ct_tmp * ct_idx_341;
    memset(&Pkk_1[65], 0, 10U * sizeof(double));
    Pkk_1[75] = (((((((Pk[75] - ct_idx_429) - ct_idx_328) - ct_idx_383) -
                    dt_Cnb[2] * t279) -
                   dt_Cnb[5] * t280) -
                  dt_Cnb[8] * t281) -
                 dt_fnsf[0] * t268) +
                dt_fnsf[1] * t267;
    Pkk_1[76] = (((((((Pk[76] - ct_idx_305) - ct_idx_381) - ct_idx_403) -
                    dt_Cnb[2] * t285) -
                   dt_Cnb[5] * t286) -
                  dt_Cnb[8] * t287) -
                 dt_fnsf[0] * t271) +
                dt_fnsf[1] * t270;
    Pkk_1[77] = (((((((Pk[77] - ct_idx_378) - ct_idx_401) - ct_idx_420) -
                    dt_Cnb[2] * t291) -
                   dt_Cnb[5] * t292) -
                  dt_fnsf[0] * t274) +
                 dt_fnsf[1] * t273) -
                dt_Cnb[8] * t293;
    e_ct_tmp = ((((Pk[78] + dt_Cnb[0] * Pk[87]) + dt_Cnb[3] * Pk[88]) +
                 dt_Cnb[6] * Pk[89]) +
                dt_fnsf[1] * Pk[77]) -
               dt_fnsf[2] * Pk[76];
    Pkk_1[78] = (((((((e_ct_tmp + dt_Cnb[2] * t298) + dt_Cnb[5] * t299) +
                     dt_Cnb[8] * t300) +
                    dt_fnsf[0] * t295) -
                   dt_fnsf[1] * t294) +
                  ct_idx_369 * Qt[3] * dt) +
                 ct_idx_373 * Qt[4] * dt) +
                ct_idx_385 * Qt[5] * dt;
    f_ct_tmp = ((((Pk[79] + dt_Cnb[1] * Pk[87]) + dt_Cnb[4] * Pk[88]) +
                 dt_Cnb[7] * Pk[89]) +
                dt_fnsf[2] * Pk[75]) -
               dt_fnsf[0] * Pk[77];
    Pkk_1[79] =
        (((((((f_ct_tmp + dt_Cnb[2] * t301) + dt_Cnb[5] * t302) +
             dt_Cnb[8] * t303) -
            dt_fnsf[1] * t297) +
           dt_fnsf[0] * (((((Pk[19] + dt_Cnb[1] * Pk[27]) + dt_Cnb[4] * Pk[28]) +
                           dt_Cnb[7] * Pk[29]) +
                          dt_fnsf[2] * Pk[15]) +
                         dt_fnsf[0] * -Pk[17])) +
          ct_idx_374 * Qt[3] * dt) +
         ct_idx_217 * Qt[4] * dt) +
        ct_idx_250 * Qt[5] * dt;
    ct_idx_369 = ((((Pk[80] + dt_Cnb[2] * Pk[87]) + dt_Cnb[5] * Pk[88]) +
                   dt_Cnb[8] * Pk[89]) +
                  dt_fnsf[0] * Pk[76]) -
                 dt_fnsf[1] * Pk[75];
    Pkk_1[80] =
        (((((((ct_idx_369 + dt_Cnb[2] * t304) + dt_Cnb[5] * t305) +
             dt_Cnb[8] * t306) -
            dt_fnsf[1] * (((((Pk[5] + dt_Cnb[2] * Pk[12]) + dt_Cnb[5] * Pk[13]) +
                            dt_Cnb[8] * Pk[14]) +
                           dt_fnsf[0] * Pk[1]) +
                          dt_fnsf[1] * -Pk[0])) +
           dt_fnsf[0] * (((((Pk[20] + dt_Cnb[2] * Pk[27]) + dt_Cnb[5] * Pk[28]) +
                           dt_Cnb[8] * Pk[29]) +
                          dt_fnsf[0] * Pk[16]) +
                         dt_fnsf[1] * -Pk[15])) +
          ct_tmp * ct_idx_342) +
         b_ct_tmp * ct_idx_343) +
        c_ct_tmp * ct_idx_344;
    memset(&Pkk_1[81], 0, 9U * sizeof(double));
    Pkk_1[90] = (((Pk[90] - dt_Cnb[0] * Pk[99]) - dt_Cnb[3] * Pk[100]) -
                 dt_Cnb[6] * Pk[101]) -
                Xkk_1_tmp * (((-Pk[60] + ct_idx_418) + ct_idx_316) + ct_idx_382);
    Pkk_1[91] = (((Pk[91] - dt_Cnb[1] * Pk[99]) - dt_Cnb[4] * Pk[100]) -
                 dt_Cnb[7] * Pk[101]) -
                Xkk_1_tmp * (((-Pk[61] + ct_idx_283) + ct_idx_379) + ct_idx_402);
    Pkk_1[92] = (((Pk[92] - dt_Cnb[2] * Pk[99]) - dt_Cnb[5] * Pk[100]) -
                 dt_Cnb[8] * Pk[101]) -
                Xkk_1_tmp * (((-Pk[62] + ct_idx_375) + ct_idx_398) + ct_idx_417);
    Pkk_1[93] = (((((Pk[93] + dt_Cnb[0] * Pk[102]) + dt_Cnb[3] * Pk[103]) +
                   dt_Cnb[6] * Pk[104]) -
                  dt_fnsf[2] * Pk[91]) +
                 dt_fnsf[1] * Pk[92]) +
                Xkk_1_tmp * ct_idx_228;
    Pkk_1[94] = (((((Pk[94] + dt_Cnb[1] * Pk[102]) + dt_Cnb[4] * Pk[103]) +
                   dt_Cnb[7] * Pk[104]) +
                  dt_fnsf[2] * Pk[90]) -
                 dt_fnsf[0] * Pk[92]) +
                Xkk_1_tmp * d_ct_tmp;
    Pkk_1[95] = (((((Pk[95] + dt_Cnb[2] * Pk[102]) + dt_Cnb[5] * Pk[103]) +
                   dt_Cnb[8] * Pk[104]) -
                  dt_fnsf[1] * Pk[90]) +
                 dt_fnsf[0] * Pk[91]) +
                Xkk_1_tmp * (((((Pk[65] + dt_Cnb[2] * Pk[72]) + dt_Cnb[5] * Pk[73]) +
                               dt_Cnb[8] * Pk[74]) -
                              dt_fnsf[1] * Pk[60]) +
                             dt_fnsf[0] * Pk[61]);
    Pkk_1[96] = (Pk[96] + Xkk_1_tmp * (Pk[66] + Mpvv[0] * Pk[64] * dt)) +
                Mpvv[0] * Pk[94] * dt;
    memset(&Pkk_1[97], 0, 8U * sizeof(double));
    Pkk_1[105] = (((Pk[105] - dt_Cnb[0] * Pk[114]) - dt_Cnb[3] * Pk[115]) -
                  dt_Cnb[6] * Pk[116]) -
                 b_Xkk_1_tmp * (((-Pk[45] + ct_idx_407) + ct_idx_294) + ct_idx_380);
    Pkk_1[106] = (((Pk[106] - dt_Cnb[1] * Pk[114]) - dt_Cnb[4] * Pk[115]) -
                  dt_Cnb[7] * Pk[116]) -
                 b_Xkk_1_tmp * (((-Pk[46] + ct_idx_261) + ct_idx_376) + ct_idx_399);
    Pkk_1[107] = (((Pk[107] - dt_Cnb[2] * Pk[114]) - dt_Cnb[5] * Pk[115]) -
                  dt_Cnb[8] * Pk[116]) -
                 b_Xkk_1_tmp * (((-Pk[47] + ct_idx_371) + ct_idx_394) + ct_idx_414);
    Pkk_1[108] = (((((Pk[108] + dt_Cnb[0] * Pk[117]) + dt_Cnb[3] * Pk[118]) +
                    dt_Cnb[6] * Pk[119]) -
                   dt_fnsf[2] * Pk[106]) +
                  dt_fnsf[1] * Pk[107]) +
                 b_Xkk_1_tmp * ct_idx_370;
    Pkk_1[109] =
        (((((Pk[109] + dt_Cnb[1] * Pk[117]) + dt_Cnb[4] * Pk[118]) +
           dt_Cnb[7] * Pk[119]) +
          dt_fnsf[2] * Pk[105]) -
         dt_fnsf[0] * Pk[107]) +
        b_Xkk_1_tmp * (((((Pk[49] + dt_Cnb[1] * Pk[57]) + dt_Cnb[4] * Pk[58]) +
                         dt_Cnb[7] * Pk[59]) +
                        dt_fnsf[2] * Pk[45]) -
                       dt_fnsf[0] * Pk[47]);
    Pkk_1[110] =
        (((((Pk[110] + dt_Cnb[2] * Pk[117]) + dt_Cnb[5] * Pk[118]) +
           dt_Cnb[8] * Pk[119]) -
          dt_fnsf[1] * Pk[105]) +
         dt_fnsf[0] * Pk[106]) +
        b_Xkk_1_tmp * (((((Pk[50] + dt_Cnb[2] * Pk[57]) + dt_Cnb[5] * Pk[58]) +
                         dt_Cnb[8] * Pk[59]) -
                        dt_fnsf[1] * Pk[45]) +
                       dt_fnsf[0] * Pk[46]);
    Pkk_1[111] = (Pk[111] + b_Xkk_1_tmp * (Pk[51] + Mpvv[0] * Pk[49] * dt)) +
                 Mpvv[0] * Pk[109] * dt;
    Pkk_1[112] = (Pk[112] + b_Xkk_1_tmp * (Pk[52] + Mpvv[1] * Pk[48] * dt)) +
                 Mpvv[1] * Pk[108] * dt;
    Pkk_1[113] = 0.0;
    Pkk_1[114] = 0.0;
    Pkk_1[115] = 0.0;
    Pkk_1[116] = 0.0;
    Pkk_1[117] = 0.0;
    Pkk_1[118] = 0.0;
    Pkk_1[119] = 0.0;
    Pkk_1[120] = (((Pk[120] - dt_Cnb[0] * Pk[129]) - dt_Cnb[3] * Pk[130]) -
                  dt_Cnb[6] * Pk[131]) -
                 c_Xkk_1_tmp * (((-Pk[75] + ct_idx_429) + ct_idx_328) + ct_idx_383);
    Pkk_1[121] = (((Pk[121] - dt_Cnb[1] * Pk[129]) - dt_Cnb[4] * Pk[130]) -
                  dt_Cnb[7] * Pk[131]) -
                 c_Xkk_1_tmp * (((-Pk[76] + ct_idx_305) + ct_idx_381) + ct_idx_403);
    Pkk_1[122] = (((Pk[122] - dt_Cnb[2] * Pk[129]) - dt_Cnb[5] * Pk[130]) -
                  dt_Cnb[8] * Pk[131]) -
                 c_Xkk_1_tmp * (((-Pk[77] + ct_idx_378) + ct_idx_401) + ct_idx_420);
    Pkk_1[123] = (((((Pk[123] + dt_Cnb[0] * Pk[132]) + dt_Cnb[3] * Pk[133]) +
                    dt_Cnb[6] * Pk[134]) -
                   dt_fnsf[2] * Pk[121]) +
                  dt_fnsf[1] * Pk[122]) +
                 c_Xkk_1_tmp * e_ct_tmp;
    Pkk_1[124] = (((((Pk[124] + dt_Cnb[1] * Pk[132]) + dt_Cnb[4] * Pk[133]) +
                    dt_Cnb[7] * Pk[134]) +
                   dt_fnsf[2] * Pk[120]) -
                  dt_fnsf[0] * Pk[122]) +
                 c_Xkk_1_tmp * f_ct_tmp;
    Pkk_1[125] = (((((Pk[125] + dt_Cnb[2] * Pk[132]) + dt_Cnb[5] * Pk[133]) +
                    dt_Cnb[8] * Pk[134]) -
                   dt_fnsf[1] * Pk[120]) +
                  dt_fnsf[0] * Pk[121]) +
                 c_Xkk_1_tmp * ct_idx_369;
    Pkk_1[126] = (Pk[126] + c_Xkk_1_tmp * (Pk[81] + Mpvv[0] * Pk[79] * dt)) +
                 Mpvv[0] * Pk[124] * dt;
    Pkk_1[127] = (Pk[127] + c_Xkk_1_tmp * (Pk[82] + Mpvv[1] * Pk[78] * dt)) +
                 Mpvv[1] * Pk[123] * dt;
    Pkk_1[128] = (Pk[128] + c_Xkk_1_tmp * (Pk[83] + Mpvv[2] * Pk[80] * dt)) +
                 Mpvv[2] * Pk[125] * dt;
    Pkk_1[129] = 0.0;
    Pkk_1[130] = 0.0;
    Pkk_1[131] = 0.0;
    Pkk_1[132] = 0.0;
    Pkk_1[133] = 0.0;
    Pkk_1[134] = 0.0;
    Pkk_1[135] = (dt_betaGyr[0] - 1.0) * t276;
    Pkk_1[136] = (dt_betaGyr[0] - 1.0) * t282;
    Pkk_1[137] = (dt_betaGyr[0] - 1.0) * t288;
    Pkk_1[138] = -(dt_betaGyr[0] - 1.0) *
                 (((((Pk[138] + dt_Cnb[0] * Pk[147]) + dt_Cnb[3] * Pk[148]) +
                    dt_Cnb[6] * Pk[149]) +
                   dt_fnsf[1] * Pk[137]) +
                  dt_fnsf[2] * -Pk[136]);
    Pkk_1[139] = -(dt_betaGyr[0] - 1.0) *
                 (((((Pk[139] + dt_Cnb[1] * Pk[147]) + dt_Cnb[4] * Pk[148]) +
                    dt_Cnb[7] * Pk[149]) +
                   dt_fnsf[2] * Pk[135]) +
                  dt_fnsf[0] * -Pk[137]);
    Pkk_1[140] = -(dt_betaGyr[0] - 1.0) *
                 (((((Pk[140] + dt_Cnb[2] * Pk[147]) + dt_Cnb[5] * Pk[148]) +
                    dt_Cnb[8] * Pk[149]) +
                   dt_fnsf[0] * Pk[136]) +
                  dt_fnsf[1] * -Pk[135]);
    Pkk_1[141] = -(dt_betaGyr[0] - 1.0) * (Pk[141] + Mpvv[0] * Pk[139] * dt);
    Pkk_1[142] = -(dt_betaGyr[0] - 1.0) * (Pk[142] + Mpvv[1] * Pk[138] * dt);
    Pkk_1[143] = -(dt_betaGyr[0] - 1.0) * (Pk[143] + Mpvv[2] * Pk[140] * dt);
    Pkk_1[144] =
        Qt[9] * dt + Pk[144] * ((dt_betaGyr[0] - 1.0) * (dt_betaGyr[0] - 1.0));
    Pkk_1[145] = 0.0;
    Pkk_1[146] = 0.0;
    Pkk_1[147] = 0.0;
    Pkk_1[148] = 0.0;
    Pkk_1[149] = 0.0;
    Pkk_1[150] = (dt_betaGyr[1] - 1.0) * t277;
    Pkk_1[151] = (dt_betaGyr[1] - 1.0) * t283;
    Pkk_1[152] = (dt_betaGyr[1] - 1.0) * t289;
    Pkk_1[153] = -(dt_betaGyr[1] - 1.0) *
                 (((((Pk[153] + dt_Cnb[0] * Pk[162]) + dt_Cnb[3] * Pk[163]) +
                    dt_Cnb[6] * Pk[164]) +
                   dt_fnsf[1] * Pk[152]) +
                  dt_fnsf[2] * -Pk[151]);
    Pkk_1[154] = -(dt_betaGyr[1] - 1.0) *
                 (((((Pk[154] + dt_Cnb[1] * Pk[162]) + dt_Cnb[4] * Pk[163]) +
                    dt_Cnb[7] * Pk[164]) +
                   dt_fnsf[2] * Pk[150]) +
                  dt_fnsf[0] * -Pk[152]);
    Pkk_1[155] = -(dt_betaGyr[1] - 1.0) *
                 (((((Pk[155] + dt_Cnb[2] * Pk[162]) + dt_Cnb[5] * Pk[163]) +
                    dt_Cnb[8] * Pk[164]) +
                   dt_fnsf[0] * Pk[151]) +
                  dt_fnsf[1] * -Pk[150]);
    Pkk_1[156] = -(dt_betaGyr[1] - 1.0) * (Pk[156] + Mpvv[0] * Pk[154] * dt);
    Pkk_1[157] = -(dt_betaGyr[1] - 1.0) * (Pk[157] + Mpvv[1] * Pk[153] * dt);
    Pkk_1[158] = -(dt_betaGyr[1] - 1.0) * (Pk[158] + Mpvv[2] * Pk[155] * dt);
    Pkk_1[159] = Pk[159] * (dt_betaGyr[0] - 1.0) * (dt_betaGyr[1] - 1.0);
    Pkk_1[160] =
        Qt[10] * dt + Pk[160] * ((dt_betaGyr[1] - 1.0) * (dt_betaGyr[1] - 1.0));
    Pkk_1[161] = 0.0;
    Pkk_1[162] = 0.0;
    Pkk_1[163] = 0.0;
    Pkk_1[164] = 0.0;
    Pkk_1[165] = (dt_betaGyr[2] - 1.0) * t278;
    Pkk_1[166] = (dt_betaGyr[2] - 1.0) * t284;
    Pkk_1[167] = (dt_betaGyr[2] - 1.0) * t290;
    Pkk_1[168] = -(dt_betaGyr[2] - 1.0) *
                 (((((Pk[168] + dt_Cnb[0] * Pk[177]) + dt_Cnb[3] * Pk[178]) +
                    dt_Cnb[6] * Pk[179]) +
                   dt_fnsf[1] * Pk[167]) +
                  dt_fnsf[2] * -Pk[166]);
    Pkk_1[169] = -(dt_betaGyr[2] - 1.0) *
                 (((((Pk[169] + dt_Cnb[1] * Pk[177]) + dt_Cnb[4] * Pk[178]) +
                    dt_Cnb[7] * Pk[179]) +
                   dt_fnsf[2] * Pk[165]) +
                  dt_fnsf[0] * -Pk[167]);
    Pkk_1[170] = -(dt_betaGyr[2] - 1.0) *
                 (((((Pk[170] + dt_Cnb[2] * Pk[177]) + dt_Cnb[5] * Pk[178]) +
                    dt_Cnb[8] * Pk[179]) +
                   dt_fnsf[0] * Pk[166]) +
                  dt_fnsf[1] * -Pk[165]);
    Pkk_1[171] = -(dt_betaGyr[2] - 1.0) * (Pk[171] + Mpvv[0] * Pk[169] * dt);
    Pkk_1[172] = -(dt_betaGyr[2] - 1.0) * (Pk[172] + Mpvv[1] * Pk[168] * dt);
    Pkk_1[173] = -(dt_betaGyr[2] - 1.0) * (Pk[173] + Mpvv[2] * Pk[170] * dt);
    Pkk_1[174] = Pk[174] * (dt_betaGyr[0] - 1.0) * (dt_betaGyr[2] - 1.0);
    Pkk_1[175] = Pk[175] * (dt_betaGyr[1] - 1.0) * (dt_betaGyr[2] - 1.0);
    Pkk_1[176] =
        Qt[11] * dt + Pk[176] * ((dt_betaGyr[2] - 1.0) * (dt_betaGyr[2] - 1.0));
    Pkk_1[177] = 0.0;
    Pkk_1[178] = 0.0;
    Pkk_1[179] = 0.0;
    Pkk_1[180] = (dt_betaAcc[0] - 1.0) * t279;
    Pkk_1[181] = (dt_betaAcc[0] - 1.0) * t285;
    Pkk_1[182] = (dt_betaAcc[0] - 1.0) * t291;
    Pkk_1[183] = -(dt_betaAcc[0] - 1.0) * t298;
    Pkk_1[184] = -(dt_betaAcc[0] - 1.0) * t301;
    Pkk_1[185] = -(dt_betaAcc[0] - 1.0) * t304;
    Pkk_1[186] = -(dt_betaAcc[0] - 1.0) * (Pk[186] + Mpvv[0] * Pk[184] * dt);
    Pkk_1[187] = -(dt_betaAcc[0] - 1.0) * (Pk[187] + Mpvv[1] * Pk[183] * dt);
    Pkk_1[188] = -(dt_betaAcc[0] - 1.0) * (Pk[188] + Mpvv[2] * Pk[185] * dt);
    Pkk_1[189] = Pk[189] * (dt_betaAcc[0] - 1.0) * (dt_betaGyr[0] - 1.0);
    Pkk_1[190] = Pk[190] * (dt_betaAcc[0] - 1.0) * (dt_betaGyr[1] - 1.0);
    Pkk_1[191] = Pk[191] * (dt_betaAcc[0] - 1.0) * (dt_betaGyr[2] - 1.0);
    Pkk_1[192] =
        Qt[12] * dt + Pk[192] * ((dt_betaAcc[0] - 1.0) * (dt_betaAcc[0] - 1.0));
    Pkk_1[193] = 0.0;
    Pkk_1[194] = 0.0;
    Pkk_1[195] = (dt_betaAcc[1] - 1.0) * t280;
    Pkk_1[196] = (dt_betaAcc[1] - 1.0) * t286;
    Pkk_1[197] = (dt_betaAcc[1] - 1.0) * t292;
    Pkk_1[198] = -(dt_betaAcc[1] - 1.0) * t299;
    Pkk_1[199] = -(dt_betaAcc[1] - 1.0) * t302;
    Pkk_1[200] = -(dt_betaAcc[1] - 1.0) * t305;
    Pkk_1[201] = -(dt_betaAcc[1] - 1.0) * (Pk[201] + Mpvv[0] * Pk[199] * dt);
    Pkk_1[202] = -(dt_betaAcc[1] - 1.0) * (Pk[202] + Mpvv[1] * Pk[198] * dt);
    Pkk_1[203] = -(dt_betaAcc[1] - 1.0) * (Pk[203] + Mpvv[2] * Pk[200] * dt);
    Pkk_1[204] = Pk[204] * (dt_betaAcc[1] - 1.0) * (dt_betaGyr[0] - 1.0);
    Pkk_1[205] = Pk[205] * (dt_betaAcc[1] - 1.0) * (dt_betaGyr[1] - 1.0);
    Pkk_1[206] = Pk[206] * (dt_betaAcc[1] - 1.0) * (dt_betaGyr[2] - 1.0);
    Pkk_1[207] = Pk[207] * (dt_betaAcc[0] - 1.0) * (dt_betaAcc[1] - 1.0);
    Pkk_1[208] =
        Qt[13] * dt + Pk[208] * ((dt_betaAcc[1] - 1.0) * (dt_betaAcc[1] - 1.0));
    Pkk_1[209] = 0.0;
    Pkk_1[210] = (dt_betaAcc[2] - 1.0) * t281;
    Pkk_1[211] = (dt_betaAcc[2] - 1.0) * t287;
    Pkk_1[212] = (dt_betaAcc[2] - 1.0) * t293;
    Pkk_1[213] = -(dt_betaAcc[2] - 1.0) * t300;
    Pkk_1[214] = -(dt_betaAcc[2] - 1.0) * t303;
    Pkk_1[215] = -(dt_betaAcc[2] - 1.0) * t306;
    Pkk_1[216] = -(dt_betaAcc[2] - 1.0) * (Pk[216] + Mpvv[0] * Pk[214] * dt);
    Pkk_1[217] = -(dt_betaAcc[2] - 1.0) * (Pk[217] + Mpvv[1] * Pk[213] * dt);
    Pkk_1[218] = -(dt_betaAcc[2] - 1.0) * (Pk[218] + Mpvv[2] * Pk[215] * dt);
    Pkk_1[219] = Pk[219] * (dt_betaAcc[2] - 1.0) * (dt_betaGyr[0] - 1.0);
    Pkk_1[220] = Pk[220] * (dt_betaAcc[2] - 1.0) * (dt_betaGyr[1] - 1.0);
    Pkk_1[221] = Pk[221] * (dt_betaAcc[2] - 1.0) * (dt_betaGyr[2] - 1.0);
    Pkk_1[222] = Pk[222] * (dt_betaAcc[0] - 1.0) * (dt_betaAcc[2] - 1.0);
    Pkk_1[223] = Pk[223] * (dt_betaAcc[1] - 1.0) * (dt_betaAcc[2] - 1.0);
    Pkk_1[224] = Qt[14] * dt + Pk[224] * ((dt_betaAcc[2] - 1.0) * (dt_betaAcc[2] - 1.0));
}
